#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "cre_moveit_forward_operation");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface arm("manipulator");

    arm.setGoalJointTolerance(0.001);

    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("grap_face");
    arm.move();
    sleep(1);

    double targetPose[5] = {0.0, 0.0, 0.74, 1.42, 0.0};
    std::vector<double> joint_group_positions(5);
    joint_group_positions[0] = targetPose[0];
    joint_group_positions[1] = targetPose[1];
    joint_group_positions[2] = targetPose[2];
    joint_group_positions[3] = targetPose[3];
    joint_group_positions[4] = targetPose[4];

    arm.setJointValueTarget(joint_group_positions);
    arm.move();
    sleep(1);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("grap_face");
    arm.move();
    sleep(1);

    ros::shutdown();

    return 0;
}